package simpleGeom;

import java.awt.geom.Line2D;

import processing.core.PGraphics;
import processing.core.PMatrix3D;
import processing.core.PVector;

public class SimpleTriangle extends AbstractFace {
	HasPos3D[] nodes=new HasPos3D[3];
	final static float SMALL_NUM=0.000001f;
	public SimpleTriangle(HasPos3D p0,HasPos3D p1, HasPos3D p2){
		nodes[0]=p0;
		nodes[1]=p1;
		nodes[2]=p2;
	}
	public HasPos3D getNode(int i){
		return nodes[i];
	}
	public int getNodesNum(){
		return 3;
	}
	public float distanceToPlane(float x, float y, float z){
		Vec3D u=Vec3D.sub(getNode(1), getNode(0));
		Vec3D v=Vec3D.sub(getNode(2), getNode(0));
		Vec3D n=Vec3D.cross(u,v);
	     n.normalize();
	     float d0=-n.dot(getNode(0));
		return n.dot(x, y, z)+d0;
}
	public void invert(){
		HasPos3D temp=this.getNode(0);
		nodes[0]=nodes[2];
		nodes[2]=temp;
		
	}
	@Override
	public float getArea() {
		// TODO Auto-generated method stub
		return  getTriangleArea(getNode(0),getNode(1),getNode(2));
	}
	public Vec3D getGravityCenter() {
		Vec3D p = new Vec3D();
		for (int i = 0; i < getNodesNum(); i++) {
			HasPos3D cP = getNode(i);
			p.add(cP);
		}
		p.mult(1f / getNodesNum());
		return p;
	}
public float getDistanceSqr(float x, float y, float z){
	return getDistanceSqr(x,y,z,new Integer(0));
}
public float getDistanceSqr(float x, float y, float z,Integer index) {
	PMatrix3D m = new PMatrix3D();
	Vec3D planeAxis = new Vec3D(0, 0, 1);
	Line3D l1;
	Line3D l2;
	Line3D l3;

	HasPos3D v1 = getNode(0);
	HasPos3D v2 = getNode(1);
	HasPos3D v3 = getNode(2);
	Vec3D v12 = Vec3D.sub(v2, v1);
	Vec3D v13 = Vec3D.sub(v3, v1);
	Vec3D n = Vec3D.cross(v12, v13);
	n.normalize();
	float angle = (float) Math.acos(n.dot(planeAxis));

	Vec3D u = n.cross(planeAxis);
	u.normalize();

	float c = (float) Math.cos(angle);
	float s = (float) Math.sin(angle);
	float t = 1f - c;

	m.m00 = c + u.getX() * u.getX() * t;
	m.m01 = u.getX() * u.getY() * t - u.getZ() * s;
	m.m02 = u.getX() * u.getZ() * t + u.getY() * s;

	m.m10 = u.getY() * u.getX() * t + u.getZ() * s;
	m.m11 = c + u.getY() * u.getY() * t;
	m.m12 = u.getY() * u.getZ() * t - u.getX() * s;

	m.m20 = u.getZ() * u.getX() * t - u.getY() * s;
	m.m21 = u.getZ() * u.getY() * t + u.getX() * s;
	m.m22 = c + u.getZ() * u.getZ() * t;

	PVector[] pts = new PVector[3];
	float[] f1 = new float[] { v1.getX(), v1.getY(), v1.getZ() };
	float[] f2 = new float[] { v2.getX(), v2.getY(), v2.getZ() };
	float[] f3 = new float[] { v3.getX(), v3.getY(), v3.getZ() };

	float[] f1T = m.mult(f1, null);
	float[] f2T = m.mult(f2, null);
	float[] f3T = m.mult(f3, null);

	l1 = new Line3D(f1T[0], f1T[1], f1T[2], f2T[0], f2T[1], f2T[2]);
	l2 = new Line3D(f2T[0], f2T[1], f2T[2], f3T[0], f3T[1], f3T[2]);
	l3 = new Line3D(f3T[0], f3T[1], f3T[2], f1T[0], f1T[1], f1T[2]);

	float minD = Float.MAX_VALUE;
	float[] pt = new float[] { x, y, z };
	float[] trans = new float[3];

	trans = m.mult(pt, null);
	float cX = trans[0];
	float cY = trans[1];
	float cZ = trans[2];


	int r1 = Line2D.relativeCCW(l1.x1, l1.y1, l1.x2, l1.y2, cX, cY);
	int r2 = Line2D.relativeCCW(l2.x1, l2.y1, l2.x2, l2.y2, cX, cY);
	int r3 = Line2D.relativeCCW(l3.x1, l3.y1, l3.x2, l3.y2, cX, cY);
	float cDistance;
	if (r1 == r2 && r1 == r3) {
		cDistance = (cZ - l1.z1) * (cZ - l1.z1);
		if (cDistance < minD) {
			minD = cDistance;
			index=-1;
		}
	} else {
		float d1 = (float) l1.ptSegDistSq(cX, cY, cZ);
		float d2 = (float) l2.ptSegDistSq(cX, cY, cZ);
		float d3 = (float) l3.ptSegDistSq(cX, cY, cZ);
		if (d1 < minD) {
			minD = d1;
			index=0;
		}
		if (d2 < minD) {
			minD = d2;
			index=1;
		}
		if (d3 < minD) {
			minD = d3;
			index=2;
		}
	}

	return (float) minD;

}
public float getDistance(float x, float y, float z) {
	return (float) Math.sqrt(getDistanceSqr(x,y,z));
}
public float getDistance(float x, float y, float z,Integer index) {
	return (float) Math.sqrt(getDistanceSqr(x,y,z,index));
}

public void display3D(PGraphics g) {
	// TODO Auto-generated method stub
	g.beginShape(PGraphics.TRIANGLES);
	for (int i = 0; i < getNodesNum(); i++) {
		HasPos3D n = getNode(i);
		g.vertex(n.getX(), n.getY(), n.getZ());
	}
	g.endShape();
}

public void display2D(PGraphics g) {
	// TODO Auto-generated method stub
	g.beginShape(PGraphics.TRIANGLES);
	for (int i = 0; i < getNodesNum(); i++) {
		HasPos3D n = getNode(i);
		g.vertex(n.getX(), n.getY());
	}
	g.endShape();
}


public float getSignedVolumeOfTriangle() {
	HasPos3D p1=nodes[0];
	HasPos3D p2=nodes[1];
	HasPos3D p3=nodes[2];
    float v321 = p3.getX()*p2.getY()*p1.getZ();
    float v231 = p2.getX()*p3.getY()*p1.getZ();
    float  v312 = p3.getX()*p1.getY()*p2.getZ();
    float v132 = p1.getX()*p3.getY()*p2.getZ();
    float v213 = p2.getX()*p1.getY()*p3.getZ();
    float v123 = p1.getX()*p2.getY()*p3.getZ();
    return (1.0f/6.0f)*(-v321 + v231 + v312 - v132 - v213 + v123);
}
public static Vec3D intersectVerticalRayTriangle( HasPos3D p0,HasPos3D p1,HasPos3D p2,HasPos3D r0 )
{

   // sameNormal
    if (Line2D.relativeCCW( p0.getX(), p0.getY(),  p1.getX(), p1.getY(),p2.getX(), p2.getY())==1){
    	HasPos3D t=p0;
    	p0=p1;
    	p1=t;
    }
    if (Line2D.relativeCCW( p2.getX(), p2.getY(),  p0.getX(), p0.getY(),r0.getX(), r0.getY())==1)return null;	   

    if (Line2D.relativeCCW( p0.getX(), p0.getY(),  p1.getX(), p1.getY(),r0.getX(), r0.getY())==1)return null;
   if (Line2D.relativeCCW( p1.getX(), p1.getY(),  p2.getX(), p2.getY(),r0.getX(), r0.getY())==1)return null;  
   return rayPlaneIntersection(p0,p1,p2,r0,new Vec3D(r0.getX(),r0.getY(),r0.getZ()+1),false);
 
}

public static Vec3D rayPlaneIntersection(HasPos3D p0,HasPos3D p1,HasPos3D p2,HasPos3D r0,HasPos3D r1,boolean seg){
	 Vec3D u=Vec3D.sub(p1, p0);
	 Vec3D v=Vec3D.sub(p2, p0);
	 Vec3D n=Vec3D.cross(u,v);
	 n.normalize();
	
	 HasPos3D dir=Vec3D.sub(r1,r0);
	 HasPos3D  w0=Vec3D.sub(r0, p0);
	   
	 double   a = -1f*Vec3D.dot3D(n,w0);
	 double    b = Vec3D.dot3D(n,dir);
	    if (Math.abs(b) < SMALL_NUM) {     // ray is parallel to triangle plane
	        if (a == 0)                // ray lies in triangle plane
	            return null;
	        else return null;             // ray disjoint from plane
	    }

	    // get intersect point of ray with triangle plane
	    double  r = a / b;
	    if (r < 0.0)                   // ray goes away from triangle
	        return null;                  // => no intersect
	    if (seg){
	    	if (r>1.0)return null;
	    }
	    // for a segment, also test if (r > 1.0) => no intersect
	  return Vec3D.add(r0, Vec3D.mult(dir,(float) r));
}
public static Vec3D linePlaneIntersection(HasPos3D p0,HasPos3D p1,HasPos3D p2,HasPos3D r0,HasPos3D r1,boolean stop1,boolean stop2){
	 Vec3D u=Vec3D.sub(p1, p0);
	 Vec3D v=Vec3D.sub(p2, p0);
	 Vec3D n=Vec3D.cross(u,v);
	 n.normalize();
	
	 HasPos3D dir=Vec3D.sub(r1,r0);
	 HasPos3D  w0=Vec3D.sub(r0, p0);
	   
	 double   a = -1f*Vec3D.dot3D(n,w0);
	 double    b = Vec3D.dot3D(n,dir);
	    if (Math.abs(b) < SMALL_NUM) {     // ray is parallel to triangle plane
	        if (a == 0)                // ray lies in triangle plane
	            return null;
	        else return null;             // ray disjoint from plane
	    }

	    // get intersect point of ray with triangle plane
	    double  r = a / b;
	    if (stop1){
	    if (r < 0.0)                   // ray goes away from triangle
	        return null;                  // => no intersect
	    }
	    if (stop2){
	    	if (r>1.0)return null;
	    }
	    // for a segment, also test if (r > 1.0) => no intersect
	  return Vec3D.add(r0, Vec3D.mult(dir,(float) r));
}

}
